#include <iostream>
#include <cstdlib>
#include <fstream>
#include <cstring>
#include <string>
#include "timetools.h"
#include <errno.h>
#include <cmath>

#include "arglib.h"


using namespace std;

vector<double*> matricesA;
vector<double*> matricesB;

int next_matrix = 0;

clarg::argInt nmats("-nm", "Number of matrix multiplications", 1);
clarg::argInt matsz("-msz", "Matrix size = N x N", 1000);
clarg::argInt nthreads("-nt", "Number of threads", 1);


double* new_matrix(double* src, unsigned N) {
  
  double *m = (double*) malloc( N*N*sizeof(double) );
  if (m == NULL) {
	printf("\n\nError: new_matrix()\n\n");
	exit( -999 );
  }

  if (src != NULL) { memcpy( m, src, N*N*sizeof(double) ); }

  else { memset( m, 0, N*N*sizeof(double) ); }
	
  return m;
}


void init_matrices(void) {
	
  for (unsigned i=0; i<nmats.get_value(); i++) {
    matricesA.push_back( new_matrix(0, matsz.get_value()) );
    matricesB.push_back( new_matrix(0, matsz.get_value()) );
  }
}


void free_matrices() {
  
  for (unsigned i=0; i<nmats.get_value(); i++) {
    free(matricesA[i]);
    free(matricesB[i]);
  }
}


int get_matrix(void) {
  int id; 

  if (next_matrix >= nmats.get_value())
    return -1;

  id = next_matrix++;

  return id;
}


void
__attribute__((noinline))
process(double * A, double * B, unsigned N)
{
    int i,j,k;

    //cholesky
    for (i=0; i<N*N; i++)
        A[i] = 1.0;

    double s;

    for (i=0; i<N; i++) {
        for (j=0; j<(i+1); j++) {
            s=0;

            for (k=0; k<j; k++)
                s += B[i * N + k] * B[j * N + k];

            if (i == j) {
                B[i * N + j] = sqrt( A[i * N + i] - s );
            }

            else {
                B[i * N + j] = ( 1.0 / B[j * N + j] * (A[i * N + j] - s) );
            }
        }
    }//for (i=0; i<N; i++)
}

void* worker_thread() {
	int matrix_id;
        
    while ( (matrix_id = get_matrix()) >= 0) {
      
		double* A = matricesA[matrix_id];
		double* B = matricesB[matrix_id];
				
		unsigned N = matsz.get_value();

		process(A, B, N);
	}
    return NULL;
}



int main(int argc, char *argv[]) {

	if (argc <= 1)
		return -10;

    if (clarg::parse_arguments(argc, argv)) {
        cerr << "Error when parsing the arguments!" << endl;
        return 1;
    }
        
    if (nmats.get_value() < 1) {
        cerr << "Error, nm must be >= 1" << endl;
        return 1;
    }

    init_matrices();

    int n_threads = nthreads.get_value();
    vector<pthread_t> threads(n_threads);
#ifdef ENABLE_OUTPUT
    timespec startTime, endTime;
    readTime(&startTime);
#endif
        worker_thread();
#ifdef ENABLE_OUTPUT
    readTime(&endTime);

    double elapsedTime = timeDiff(startTime, endTime);
    printf("RESULT %d\nTIME %f\n", matsz.get_value(), elapsedTime);
#endif
	free_matrices();

	return 0;
}
